package com.zhdl.modules.assemblybolt.service.impl;

import com.project.common.util.DelayUtil;
import com.zhdl.common.eventbus.center.EventBusCenter;
import com.zhdl.common.ytools.HexToBitTools;
import com.zhdl.hardware.clamping_jaw.inspire.service.InspireSendCmdService;
import com.zhdl.hardware.clamping_jaw.micro.service.MicroSendCmdService;
import com.zhdl.hardware.plc.siemens.service.SiemensCmdSendService;
import com.zhdl.hardware.robot.kuka.service.KukaCmdSendService;
import com.zhdl.modules.assemblybolt.web.req.AssemblyBoltData;
import com.zhdl.modules.assemblybolt.web.req.KuKaRobotCmd;
import com.zhdl.modules.assemblybolt.service.AssemblyBoltService;
import com.zhdl.modules.tools.CommonCallback;
import com.zhdl.modules.tools.IntToHexUtil;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.concurrent.ThreadPoolTaskScheduler;
import org.springframework.stereotype.Service;

import javax.annotation.Resource;
import java.util.HashMap;
import java.util.Map;
import java.util.concurrent.ScheduledFuture;
import java.util.concurrent.atomic.AtomicReference;

/**
 * 双头螺杆运动
 */
@Slf4j
@Service
public class AssemblyBoltServiceImpl implements AssemblyBoltService {

    private static final String plcIp = "192.168.1.30:502";
    @Resource
    public ThreadPoolTaskScheduler taskScheduler1;
    Map<String, ScheduledFuture<?>> mapSF = new HashMap<>();
    /**
     * 库卡机器人指令参数
     */
    KuKaRobotCmd kuKaRobotCmd = new KuKaRobotCmd();
    String siemensValue =
            "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000" +
                    "0000000000000000000000000000000000000000" + "0000000000000000000000000000000000000000";
    @Resource
    private KukaCmdSendService kukaCmdSendService;
    @Resource
    private SiemensCmdSendService siemensCmdSendService;
    @Resource
    private InspireSendCmdService inspireSendCmdService;
    @Resource
    private MicroSendCmdService microSendCmdService;

    /**
     * 将byte转换为一个长度为8的byte数组，数组每个值代表bit
     */
    public static Byte[] getBitArray(byte b) {
        Byte[] array = new Byte[8];
        for (int i = 7; i >= 0; i--) {
            array[i] = (byte) (b & 1);
            b = (byte) (b >> 1);
        }
        return array;
    }

    /**
     * PLC-获取电机寄存器地址
     * 轴一 write 0-34 read 34-48
     *
     * @param model 电机型号
     * @return 寄存器地址
     */
    public String motorModelAddress(int model) {
        int addDec = (model - 1) * 24;
        return IntToHexUtil.shortToHex(addDec);
    }

    /**
     * 寄存器-地址变大操作
     *
     * @param hexAddress 十六进制寄存器地址
     * @param upNum      要增大的数字
     * @return 十六进制地址
     */
    public String hexAddressUp(String hexAddress, int upNum) {
        int addressDec = Integer.parseInt(hexAddress, 16);
        upNum = upNum / 2;
        addressDec = addressDec + upNum;
        return IntToHexUtil.shortToHex(addressDec);
    }

    /**
     * 库卡机器人-指令控制
     *
     * @param instruct 指令编号
     * @param callback 回调
     */
    @Override
    public void kukaSendCmd(int instruct, CommonCallback<String> callback) {
        String cmd;
        switch (instruct) {
            case 1 -> {
                cmd = kuKaRobotCmd.getPickUp();
            }
            case 2 -> {
                cmd = kuKaRobotCmd.getPickDown();
            }
            case 3 -> {
                cmd = "";
                log.info("当前为空指令，预留指令位");
            }
            case 4 -> {
                cmd = kuKaRobotCmd.getJigClose();
            }
            case 5 -> {
                cmd = kuKaRobotCmd.getJigOpen();
            }
            case 6 -> {
                cmd = kuKaRobotCmd.getTransportDown();
            }
            case 7 -> {
                cmd = kuKaRobotCmd.getJigClose2();
            }
            case 8 -> {
                cmd = kuKaRobotCmd.getTransportPutDown();
            }
            case 9 -> {
                cmd = kuKaRobotCmd.getJigOpen2();
            }
            default -> cmd = "";
        }
        /*
            库卡机器人-发送指令
         */
        kukaCmdSendService.kukaRobotSendCmd(cmd, s -> {
            callback.statusCallback(s);
            log.info("库卡机器人-运动完成");
        });
    }

    /**
     * PLC-读取所有寄存器
     */
    @Override
    public void PlcRead0To400(CommonCallback<String> callback) {
        AtomicReference<String> before = new AtomicReference<>("");
        AtomicReference<String> after = new AtomicReference<>("");
        siemensCmdSendService.siemensAddressValue(plcIp, "0000", 101, 400,s -> {
            before.set(s);
            if (!before.get().isEmpty() && !after.get().isEmpty()) {
                siemensValue = before.get() + after.get();
                callback.statusCallback(siemensValue);
                AssemblyBoltData assemblyBoltData = new AssemblyBoltData();
                assemblyBoltData.setSiemensValue0To404(siemensValue);
                EventBusCenter.getEventBus().post(assemblyBoltData);
            }
            log.info("PLC 0To240 读出的值为:" + s);
        });
        DelayUtil.delayedExecution(10, () ->
                siemensCmdSendService.siemensAddressValue(plcIp, "0065", 101, 400, s -> {
                    after.set(s);
                    if (!before.get().isEmpty() && !after.get().isEmpty()) {
                        siemensValue = before.get() + after.get();
                        callback.statusCallback(siemensValue);
                        AssemblyBoltData assemblyBoltData = new AssemblyBoltData();
                        assemblyBoltData.setSiemensValue0To404(siemensValue);
                        EventBusCenter.getEventBus().post(assemblyBoltData);
                    }
                    log.info("PLC 240To404 读出的值为:" + s);
                }));
    }

    /**
     * PLC-设置电机速度
     *
     * @param model     轴编号
     * @param speedType 速度类型
     * @param speed     速度
     * @param callback  回调
     */
    @Override
    public void plcSetMotorSpeed(int model, int speedType, float speed, CommonCallback<String> callback) {
        String address = motorModelAddress(model);
        /* 6 绝对位置速度 14 相对位置速度 18相对运动速度 */
        String hexAddressUp = "";
        if (speedType == 0) {
            //绝对
            hexAddressUp = hexAddressUp(address, 6);
        } else if (speedType == 1) {
            //相对
            hexAddressUp = hexAddressUp(address, 14);
        } else if (speedType == 2) {
            //job
            hexAddressUp = hexAddressUp(address, 18);
        }
        String hexSpeed = IntToHexUtil.floatToHex(speed);
        StringBuilder runHex = new StringBuilder(hexSpeed);
        int len = runHex.length();
        if (runHex.length() < 8) {
            for (int i = 0; i < 8 - len; i++) {
                runHex.insert(0, "0");
            }
        }
        siemensCmdSendService.siemensWriteValues(plcIp, hexAddressUp, runHex.toString());
    }

    /**
     * PLC-设置电机距离
     *
     * @param model        轴编号
     * @param distanceType 距离类型
     * @param distance     速度
     * @param callback     回调
     */
    @Override
    public void plcSetMotorDistance(int model, int distanceType, float distance, CommonCallback<String> callback) {
        String address = motorModelAddress(model);
        /* 2 绝对位置距离 10 相对位置速度 */
        String hexAddress = "";
        if (distanceType == 0) {
            hexAddress = hexAddressUp(address, 2);
        } else if (distanceType == 1) {
            hexAddress = hexAddressUp(address, 10);
        }
        String hexDistance = IntToHexUtil.floatToHex(distance);
        StringBuilder runHex = new StringBuilder(hexDistance);
        int len = runHex.length();
        if (runHex.length() < 8) {
            for (int i = 0; i < 8 - len; i++) {
                runHex.insert(0, "0");
            }
        }
        siemensCmdSendService.siemensWriteValues(plcIp, hexAddress, runHex.toString());
    }

    /**
     * PLC-设置电机运动
     *
     * @param model     轴编号
     * @param readyType 使能类型
     * @param value     写入值
     * @param callback  回调
     */
    @Override
    public void plcSetMotorReady(int model, int readyType, int value, CommonCallback<String> callback) {
        String address = motorModelAddress(model);
        if (address.isEmpty()) {
            return;
        }
        /* 0 绝对位置运动 0.1 相对位置运动 0.2 0.3 job 正向反向运动 */
        StringBuffer hexRun = new StringBuffer();
        if (readyType == 0) {
            hexRun.append(HexToBitTools.writeValueByBit("0000", 0, value));
        } else if (readyType == 1) {
            hexRun.append(HexToBitTools.writeValueByBit("0000", 1, value));
        }
        /* 设置安全位置 */
        StringBuffer safe = hexRun.replace(2, 4, "01");
        String hexValue = safe.toString();
        siemensCmdSendService.siemensWriteValue(plcIp, address, hexValue);
    }

    /**
     * 控制电机找原点
     *
     * @param model 电机编号
     * @param callback 回调
     */
    @Override
    public void plcSetMotorHomeTrig(int model,CommonCallback<String> callback){
        String address = motorModelAddress(model);
        if (address.isEmpty()) {
            return;
        }
        /* 设置安全位置 + 回原点 */
        String value = "0003";
        siemensCmdSendService.siemensWriteValue(plcIp,address,value);
    }

    /**
     * PLC-设置电机运动
     *
     * @param model     轴编号
     * @param readyType 使能类型
     * @param value     写入值
     * @param status    正传/反转
     * @param callback  回调
     */
    @Override
    public void plcSetMotorReady(int model, int readyType, int value, boolean status, CommonCallback<String> callback) {
        String address = motorModelAddress(model);
        if (address.isEmpty()) {
            return;
        }
        /* 0 绝对位置运动 0.1 相对位置运动 0.2 0.3 job 正向反向运动 */
        StringBuffer hexRun = new StringBuffer();
        if (readyType == 2) {
            if (status) {
                hexRun.append(HexToBitTools.writeValueByBit("0000", 2, value));
            } else {
                hexRun.append(HexToBitTools.writeValueByBit("0000", 3, value));
            }
        }
        /* 设置安全位置 */
        StringBuffer safe = hexRun.replace(2, 4, "01");
        String hexValue = safe.toString();
        siemensCmdSendService.siemensWriteValue(plcIp, address, hexValue);
    }

    /**
     * PLC-读取电机是否运动到位
     *
     * @param model    电机编号
     * @param callback 回调
     */
    @Override
    public void readPlcMotorRunDone(int model, CommonCallback<Boolean> callback) {
        String address = motorModelAddress(model);
        if (address.isEmpty()) {
            return;
        }
        /* 一直读取是否到位 */
        DelayUtil.delayedExecution(100, () -> {
            ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() -> {
                String hexAddressUp = hexAddressUp(address, 34);
                String valueByAddress = HexToBitTools.readHexValueByAddress(siemensValue, hexAddressUp);
                String bitByValue = HexToBitTools.readBitValueByHex(valueByAddress);
                String doneStatus = bitByValue.substring(0, 1);
                log.info("Motor-" + model + "-当前编号电机状态"+doneStatus) ;
                if (doneStatus.equals("1")) {
                    log.info("Motor-" + model + "-当前编号电机运动完成");
                    ScheduledFuture<?> scheduledFuture1 = mapSF.get("Motor编号" + model);
                    if (scheduledFuture1!=null){
                        scheduledFuture1.cancel(true);
                    }
                    callback.statusCallback(true);
                }
            }, 100);
            mapSF.put("Motor编号" + model, scheduledFuture);
        });
    }

    /**
     * PLC-判断DI是否到位
     *
     * @param bit      plc地址
     * @param callback 回调
     */
    @Override
    public void readMotorIOStatus(int bit, CommonCallback<Boolean> callback) {
        /* 384 PLC DI 数据*/
        String address = "00C0";
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() -> {
            /* DI Hex 数据 */
            String diValue = HexToBitTools.readHexValueByAddress(siemensValue, address);
            /* DI数据 */
            String bitByValue = HexToBitTools.readBitValueByHex(diValue);
            /* 获取第几位数据 */
            String bitValue = bitByValue.substring(bit, bit + 1);
            /* 判断当前是否为1 */
            if (bitValue.equals("1")) {
                log.info("IO-当前编号电机运动完成");
                ScheduledFuture<?> scheduledFuture1 = mapSF.get("IO信号");
                scheduledFuture1.cancel(true);
                callback.statusCallback(true);
            }
        }, 100);
        mapSF.put("IO信号", scheduledFuture);
    }


    /**
     * AI测距
     *
     * @param part 0 机器人 1 螺杆
     * @param callback 回调
     */
    @Override
    public void readMotorAIDistance(int part, CommonCallback<String> callback){
        String address = "";
        if (part == 0){
            /* 388 PLC DI 数据*/
            address = "00C2";
        } else {
            /* 392 PLC DI 数据*/
            address = "00C4";
        }
        String distance = HexToBitTools.readHexValueByAddress(siemensValue, address)+HexToBitTools.readHexValueByAddress(siemensValue, "00C2".equals(address)?"00C3":"00C5");
        callback.statusCallback(distance);
    }

    /**
     * PLC-写入DO控制
     *
     * @param bit      plc地址
     * @param value    地址值
     * @param callback 回调
     */
    @Override
    public void writeMotorIOStatus(int bit, int value, CommonCallback<Boolean> callback) {
        /* 386 PLC DO 数据*/
        String address = "00C1";
        /* DI Hex 数据 */
        String diValue = HexToBitTools.readHexValueByAddress(siemensValue, address);
        /* DI数据 */
//        String bitByValue = HexToBitTools.readBitValueByHex(diValue);
        /* 修改后的 hexDI数据 */
        String hexValue = HexToBitTools.writeValueByBit(diValue, bit, value);
        siemensCmdSendService.siemensWriteValue(plcIp, address, hexValue);
        callback.statusCallback(true);
    }

    /**
     * 控制 因时 电缸伸出
     *
     * @param port     因时串口
     * @param instruct 指令编号
     * @param callback 回调
     */
    @Override
    public void controlInspireMotorRun(String port, int instruct, CommonCallback<String> callback) {
        inspireSendCmdService.inspireGenericCmd(port, instruct, s -> {
            log.info("电机开始运动" + s);
            callback.statusCallback("ok");
        });
    }

    /**
     * 控制 micro电机运动
     *
     * @param port     micro串口
     * @param instruct 指令编号
     * @param callback 回调
     */
    @Override
    public void controlMicroMotorRun(String port, int model, int instruct, CommonCallback<String> callback) {
        microSendCmdService.microGenericCmd(port, model, instruct, s -> {
            log.info("电机开始运动" + s);
            callback.statusCallback("ok");
        });
    }


    /**
     * 点动-JOG运动
     *
     * @param model  电机型号
     * @param speed  速度
     * @param status 正反转
     * @param open   运动/停止
     */
    @Override
    public void plcControlMotorJobRun(int model, float speed, boolean status, boolean open) {
        int value = open ? 1 : 0;
        plcSetMotorSpeed(model, 2, speed, s ->
                log.info("开始运动")
                );
        plcSetMotorReady(model, 2, value, status, s1 -> {
            log.info("当前电机编号为:" + model + "当前状态为:" + status + "运动状态为:" + value);
        });
    }

    /**
     * 使能 - 绝对位置使能
     *
     * @param model 电机编号
     * @param open  打开 关闭
     */
    @Override
    public void plcControlMotorAbsRun(int model, boolean open) {
        int value = open ? 1 : 0;
        plcSetMotorReady(model, 0, value, s -> {
            log.info("电机编号为" + model + "的电机使能完成");
        });
    }

    /**
     * 使能 - 相对位置使能
     *
     * @param model 电机编号
     * @param open  打开 关闭
     */
    @Override
    public void plcControlMotorSerRun(int model, boolean open) {
        int value = open ? 1 : 0;
        plcSetMotorReady(model, 1, value, s -> {
            log.info("电机编号为" + model + "的电机使能完成");
        });
    }

    /**
     * 设置-绝对运动电机运动距离和速度
     *
     * @param model    电机编号
     * @param speed    速度
     * @param distance 距离
     */
    @Override
    public void plcControlMotorRunX(int model, float speed, float distance) {
        plcSetMotorSpeed(model, 0, speed, s -> {
            log.info("设置编号为:" + model + "电机速度完成，值为:" + speed);
        });
        plcSetMotorDistance(model, 0, distance, s -> {
            log.info("设置编号为:" + model + "电机距离完成，值为:" + distance);
        });
    }

    /**
     * 设置-绝对运动电机运动距离和速度
     *
     * @param model    电机编号
     * @param speed    速度
     * @param distance 距离
     */
    @Override
    public void plcControlMotorRunY(int model, float speed, float distance) {
        plcSetMotorSpeed(model, 1, speed, s -> {
            log.info("设置编号为:" + model + "电机速度完成，值为:" + speed);
        });
        plcSetMotorDistance(model, 1, distance, s -> {
            log.info("设置编号为:" + model + "电机距离完成，值为:" + distance);
        });
    }



    //    @Override
//    public void plcControlMotorRunX(int model, float speed, float distance) {
//        String address = motorModelAddress(model);
//        if (address.isEmpty()) {
//            return;
//        }
//        int addressDec = Integer.parseInt(address, 16);
//        addressDec = addressDec + 1;
//        address = IntToHexUtil.shortToHex(addressDec);
//
//        /* 设置电机运行距离 */
//        StringBuilder runHex = new StringBuilder(IntToHexUtil.floatToHex(distance * 1.0f));
//        int len = runHex.length();
//        if (runHex.length()<8){
//            for (int i = 0; i < 8-len; i++) {
//                runHex.insert(0, "0");
//            }
//        }
//        siemensCmdSendService.siemensWriteValues(plcIp, address, runHex.toString());
//    }

    /**
     * plc控制电机运动-绝对位置
     *
     * @param model    第几个电机
     * @param speed    速度
     * @param distance 距离
     */
//    @Override
//    public void plcControlMotorRun(int model, int speed, int distance) {
//        String address = motorModelAddress(model);
//        if (address.isEmpty()) {
//            return;
//        }
//        int addressDec = Integer.parseInt(address, 16);
//        addressDec = addressDec + 1;
//        address = IntToHexUtil.shortToHex(addressDec);
//
//        /* 设置电机运行距离 */
//        StringBuilder runHex = new StringBuilder(IntToHexUtil.floatToHex(distance * 1.0f));
//        int len = runHex.length();
//        if (runHex.length()<8){
//            for (int i = 0; i < 8-len; i++) {
//                runHex.insert(0, "0");
//            }
//        }
//        siemensCmdSendService.siemensWriteValues(plcIp, address, runHex.toString());
//    }

    /*
       JOb电动
 */
//        String address = motorModelAddress(model);
//        if (address.isEmpty()) {
//            return;
//        }
//        /* 设置正反转 以及是否运行 */
//        StringBuffer runHex = new StringBuffer();
//        if (status) {
//            runHex.append(HexToBitTools.writeValueByBit("0000", 2, open ? 1 : 0));
//        } else {
//            runHex.append(HexToBitTools.writeValueByBit("0000", 3, open ? 1 : 0));
//        }
//        StringBuffer replace = runHex.replace(2, 4, "01");
//        String hexValue = replace.toString();
//        siemensCmdSendService.siemensWriteValue(plcIp, address, hexValue);

/*
    绝对位置运动
 */
    //        String address = motorModelAddress(model);
//        if (address.isEmpty()) {
//            return;
//        }
//        /* 设置正反转 以及是否运行 */
//        StringBuffer runHex = new StringBuffer();
//        if (open) {
//            runHex.append(HexToBitTools.writeValueByBit("0000", bit, 1));
//        } else {
//            runHex.append(HexToBitTools.writeValueByBit("0000", bit, 0));
//        }
//        StringBuffer replace = runHex.replace(2, 4, "01");
//        String hexValue = replace.toString();
//        siemensCmdSendService.siemensWriteValue(plcIp, address, hexValue);

}



